/*
 * @Description: stream data reactor implementation
 * @Version: V1.0
 * @Author: zw_1520@163.com
 * @Date: 2024-09-09 14:19:43
 * @LastEditors: zw_1520@163.com
 * @LastEditTime: 2025-05-14 16:50:26
 * Copyright (C) All rights reserved.
 */
#include "stream_data_reactor.h"

#include <json/json.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2/LinearMath/Quaternion.h>

#include "robot_grpc_common.h"

/**
 * @description: privide for external module to send data
 * @param {RobotResponse&} data
 * @return {*}
 * @author: zw_1520@163.com
 */
void RobotControllerServiceImpl::sendData(const RobotResponse& data) {
  // Gets lock to push reply data
  std::lock_guard<std::mutex> locker(this->reply_queue_mutex_);
  reply_queue_.push(data);
}

/**
 * @description: request process thread function entry
 * @return {*}
 * @author: zw_1520@163.com
 */
void RobotControllerServiceImpl::requestProcessThreadEntry() {
  while (true) {
    if (!request_queue_.empty()) {
      RobotRequest robot_request;
      {
        // Gets lock to pop request data
        std::lock_guard<std::mutex> locker(request_queue_mutex_);
        if (request_queue_.size() <= 0) {
          continue;
        }
        robot_request = request_queue_.front();
        request_queue_.pop();
      }
      // Processes request
      processRequest(robot_request);
    }
    std::this_thread::sleep_for(std::chrono::milliseconds(1));
  }
}

/**
 * @description: process request
 * @param {RobotRequest&} robot_request
 * @return {*}
 * @author: zw_1520@163.com
 */
void RobotControllerServiceImpl::processRequest(RobotRequest& robot_request) {
  RCLCPP_INFO(rclcpp::get_logger("stream_data_reactor"),
              "ProcessRequest enter.");
  int msg_id = 1;
  try {
    msg_id = std::stoi(robot_request.msg_id());
  } catch (const std::exception& e) {
    RCLCPP_INFO(rclcpp::get_logger("stream_data_reactor"),
                "Error converting string to int: %s.", e.what());
    return;
  }
  RCLCPP_INFO(rclcpp::get_logger("stream_data_reactor"),
              "ProcessRequest msg_id: %d.", msg_id);
  switch (msg_id) {
    case TASK_CONTROL:
      taskControl(robot_request.data());
      break;
    default:
      RCLCPP_INFO(rclcpp::get_logger("stream_data_reactor"),
                  "Unsupport msg msg_id: %d, data: %s.", msg_id,
                  robot_request.data().c_str());
      break;
  }
}

/**
 * @description: process robot task control message
 * @param {string} msg
 * @return {*}
 * @author: zw_1520@163.com
 */
bool RobotControllerServiceImpl::taskControl(std::string msg) {
  Json::Value root;
  Json::Reader reader;
  bool parser_result = reader.parse(msg, root);
  if (!parser_result) {
    RCLCPP_ERROR(rclcpp::get_logger("stream_data_reactor"),
                 "Failed to parse JSON string: %s.",
                 reader.getFormattedErrorMessages().c_str());
    return false;
  }

  RCLCPP_INFO(rclcpp::get_logger("stream_data_reactor"),
              "Motion control message: %s", msg.c_str());

  if (root.isMember("parameters")) {
    Json::Value parameters = root["parameters"];
    if (parameters.isMember("task_type")) {
      if (parameters["task_type"].asString() == "play_motion") {
        RCLCPP_INFO(rclcpp::get_logger("stream_data_reactor"),
                    "Play motion execute.");
        if (parameters.isMember("motion_id")) {
          // Constructs object detection request
          auto request =
              std::make_shared<robot_interface::srv::SetRobotTask::Request>();
          auto message = robot_interface::msg::RobotTaskInfo();
          message.task_type = "play_motion";
          message.task_param = parameters["motion_id"].asString();
          RCLCPP_INFO(rclcpp::get_logger("stream_data_reactor"),
                      "Motion id: %s",
                      parameters["motion_id"].asString().c_str());
          agibot_task_control_publisher_->publish(message);
        } else {
          RCLCPP_ERROR(rclcpp::get_logger("stream_data_reactor"),
                       "Motion id is not exist.");
        }
      } else if (parameters["task_type"].asString() == "tts" &&
                 parameters.isMember("tts_value")) {
        auto message = robot_interface::msg::RobotTaskInfo();
        message.task_type = "tts";
        message.task_param = parameters["tts_value"].asString();
        RCLCPP_INFO(rclcpp::get_logger("stream_data_reactor"), "Tts value: %s",
                    parameters["tts_value"].asString().c_str());
        agibot_task_control_publisher_->publish(message);
      } else if (parameters["task_type"].asString() == "navigation" &&
                 parameters.isMember("poi")) {
        auto message = robot_interface::msg::RobotTaskInfo();
        message.task_type = "navigation";
        Json::FastWriter fw;
        std::string robot_navi_message = fw.write(parameters["poi"]);
        std::cout << "robot_navi_message: " << robot_navi_message << std::endl;

        message.task_param = robot_navi_message;
        RCLCPP_INFO(rclcpp::get_logger("stream_data_reactor"), "poi value: %s",
                    robot_navi_message.c_str());
        agibot_task_control_publisher_->publish(message);
      }
      // Sends reply
      RobotResponse reply_msg;
      std::string robot_reply_message;
      Json::FastWriter fw;
      Json::Value reply_root;

      reply_root["msg_id"] = "1002";
      reply_root["msg_name"] = root["msg_name"];
      reply_root["from"] = "robot";
      reply_root["to"] = root["from"];
      reply_root["parameters"]["task_type"] = parameters["task_type"];
      reply_root["parameters"]["result"] = 1;

      robot_reply_message = fw.write(reply_root);

      reply_msg.set_msg_id("1002");
      reply_msg.set_data(robot_reply_message);
      this->sendData(reply_msg);
      RCLCPP_INFO(rclcpp::get_logger("stream_data_reactor"),
                  "Robot reply message: %s.", robot_reply_message.c_str());
      return true;
    }
  }
  return false;
}

void RobotControllerServiceImpl::playMotionResponse(
    rclcpp::Client<robot_interface::srv::SetRobotTask>::SharedFuture future) {
  RCLCPP_INFO(rclcpp::get_logger("stream_data_reactor"),
              "Play motion execute success.");
}
